#include "memory/memory.h"
#include "pico/mutex.h"
#include <string.h>

// #define SELF_TEST

#define BLOCK_SIZE 4
#define BLOCK_COUNT (ALLOC_BASE_SIZE / BLOCK_SIZE)

static mutex_t lock_;  // 锁
static UInt32 memory_[BLOCK_COUNT];
static UInt32 *memory_end_ = &memory_[BLOCK_COUNT - 1];
static UInt32 *last_block_adr_ = NULL;


void TMyMemoryCreate(void)
{
    UInt32 block_info;

    mutex_init(&lock_);

    memset(memory_, 0x00, sizeof(memory_));

    block_info = BLOCK_COUNT - 2;

    // 填充内存块信息
    *memory_ = block_info;
    *memory_end_ = block_info;

    last_block_adr_ = memory_;
}

void TMyMemoryDestroy(void)
{

}

#ifdef SELF_TEST
#include <stdio.h>
static void print_memory()
{
    for (Int32 i = 0; i < BLOCK_COUNT / 16; i ++)
    {
        for (Int32 j = 0; j < 16; j ++)
        {
            Int32 idx = i * 16 + j;

            printf("\033[44m0x%08X \033[44m", &memory_[idx]);
        }
        printf("\033[40m\n");
        for (Int32 j = 0; j < 16; j ++)
        {
            Int32 idx = i * 16 + j;

            if (memory_[idx] > 0)
                printf("\033[33m0x%08X \033[37m", memory_[idx]);
            else
                printf("0x%08X ", memory_[idx]);
        }
        printf("\n");
    }
}
#endif

static void* do_alloc(UInt32 *start, UInt32 *end, UInt32 size)
{
    void *ret_adr = NULL;

    UInt32 *look_adr   = start;
    UInt32  block_need = size / BLOCK_SIZE + (size % BLOCK_SIZE > 0 ? 1 : 0);
    UInt32  block_size = 0;
    Bool    block_used = False;

    mutex_enter_blocking(&lock_);
    while (look_adr < end)  ///< 从之前返回的内存块找到尾
    {
        block_used = (*look_adr) >> 31;
        block_size = (*look_adr) & 0x7FFFFFFF;

        if (! block_used && block_size >= block_need)  ///< 找到了
        {
            UInt32 left_size = block_size - block_need;

            if (left_size > 2)  ///< 多出来的内存值得拆分
            {
                UInt32 new_block_size = left_size - 2;  ///< 新内存块的大小

                // 更新当前内存块的信息
                *look_adr = block_need | 0x80000000;
                *(look_adr + block_need + 1) = *look_adr;
                last_block_adr_ = look_adr;

                // 设置新内存块的信息
                look_adr += block_need + 2;  ///< 移到新的内存块
                *look_adr = new_block_size;
                *(look_adr + new_block_size + 1) = *look_adr;
            }
            else  ///< 多出来的内存不值得拆分
            {
                // 更新当前内存块的信息
                *look_adr = block_size | 0x80000000;
                *(look_adr + block_size + 1) = *look_adr;
                last_block_adr_ = look_adr;
            }

            ret_adr = last_block_adr_ + 1;
            break;
        }

        look_adr += block_size + 2;  ///< 前往下一个内存块
    }
    mutex_exit(&lock_);

    return ret_adr;
}

void* MyMalloc(UInt32 size)
{
    void *ret_adr = NULL;

    if (size > 0)
    {
        // 先从上次返回的内存块开始找
        ret_adr = do_alloc(last_block_adr_, memory_end_, size);

        if (ret_adr == NULL)  ///< 没找到，再从头找到之前返回的内存块
        {
            ret_adr = do_alloc(memory_, last_block_adr_, size);
        }

    }

    return ret_adr;
}

void MyFree(void *space)
{
    mutex_enter_blocking(&lock_);
    if ((UInt32 *)space > memory_ && (UInt32 *)space < memory_end_)
    {
        UInt32 *block_adr = (UInt32 *)space - 1;

        // 更新当前内存块的信息
        *block_adr &= 0x7FFFFFFF;
        *(block_adr + *block_adr + 1) = *block_adr;

        // 尝试合并下个内存块
        UInt32 *next_adr = block_adr + *block_adr + 2;
        if (next_adr < memory_end_ && (*next_adr) >> 31 == 0)  ///< 下个内存块空闲，开始合并
        {
            *block_adr += *next_adr + 2;
            *(block_adr + *block_adr + 1) = *block_adr;
            *next_adr = 0;
            *(next_adr - 1) = 0;
        }

        last_block_adr_ = block_adr;

        // 尝试合并上个内存块
        UInt32 *prev_adr = block_adr - 1;
        if (prev_adr > memory_ && (*prev_adr) >> 31 == 0)  ///< 上个内存块空闲，开始合并
        {
            // 移到上个内存块的头部
            prev_adr -= (*prev_adr) + 1;

            // 更新内存块信息
            *prev_adr += *block_adr + 2;
            *(prev_adr + *prev_adr + 1) = *prev_adr;

            *block_adr = 0;
            *(block_adr - 1) = 0;

            last_block_adr_ = prev_adr;
        }
    }
    mutex_exit(&lock_);
}

#ifdef SELF_TEST
int main(void)
{
    TMyMemoryCreate();

    print_memory();

    // // test1. 连续分配内存，观察内存块变化
    // printf("malloc %d, ret=%p\n", 16, MyMalloc(16));
    // print_memory();
    // printf("malloc %d, ret=%p\n", 17, MyMalloc(17));
    // print_memory();
    // printf("malloc %d, ret=%p\n", 1, MyMalloc(1));
    // print_memory();
    // // test2. 连续分配、释放内存，观察内存块变化
    // void *p = NULL;
    // p = MyMalloc(16);
    // printf("malloc %d, ret=%p\n", 16, p);
    // print_memory();
    // printf("free %p\n", p);
    // MyFree(p);
    // print_memory();
    // p = MyMalloc(16);
    // printf("malloc %d, ret=%p\n", 16, p);
    // print_memory();
    // printf("free %p\n", p);
    // MyFree(p);
    // print_memory();
    // // test3. 连续分配、释放中间的内存，观察内存块变化
    // void *p1, *p2, *p3 = NULL;
    // p1 = MyMalloc(16);
    // printf("malloc %d, ret=%p\n", 16, p1);
    // print_memory();
    // p2 = MyMalloc(16);
    // printf("malloc %d, ret=%p\n", 16, p2);
    // print_memory();
    // p3 = MyMalloc(16);
    // printf("malloc %d, ret=%p\n", 16, p3);
    // print_memory();
    // printf("free %p\n", p2);
    // MyFree(p2);
    // print_memory();
    // p2 = MyMalloc(16);
    // printf("malloc %d, ret=%p\n", 16, p2);
    // print_memory();
    // // test4. 连续分配、释放两端的内存，再释放中间内存，观察内存块变化
    // void *p1, *p2, *p3 = NULL;
    // p1 = MyMalloc(16);
    // printf("malloc %d, ret=%p\n", 16, p1);
    // print_memory();
    // p2 = MyMalloc(16);
    // printf("malloc %d, ret=%p\n", 16, p2);
    // print_memory();
    // p3 = MyMalloc(16);
    // printf("malloc %d, ret=%p\n", 16, p3);
    // print_memory();
    // printf("free %p\n", p1);
    // MyFree(p1);
    // print_memory();
    // printf("free %p\n", p3);
    // MyFree(p3);
    // print_memory();
    // printf("free %p\n", p2);
    // MyFree(p2);
    // print_memory();
    // test5. 连续分配1字节，直到分配失败，观察内存块变化
    void *p = NULL;
    while (p = MyMalloc(1))
    {
        printf("malloc 1\n");
        print_memory();
    }

    return 0;
}
#endif
